package math.vector;

import math.Maths;

public class vec3f 
{
    
    private static final float equiv = 1E-4f;
    
    //Instance vars
    public float x;
    public float y;
    public float z;

    //Constructors
    public vec3f() 
    {
        x = 0;
        y = 0;
        z = 0;
    }

    public vec3f(float cx, float cy, float cz) 
    {
        x = cx;
        y = cy;
        z = cz;
    }

    public vec3f(double cx, double cy, double cz) 
    {
        x = (float) cx;
        y = (float) cy;
        z = (float) cz;
    }

    public vec3f(float[] xyz) 
    {
        x = xyz[0];
        y = xyz[1];
        z = xyz[2];
    }

    public vec3f(float[][] xyz) 
    {
        x = xyz[0][0];
        y = xyz[0][1];
        z = xyz[0][2];
    }

    public vec3f(vec3f u) 
    {
        x = u.x;
        y = u.y;
        z = u.z;
    }

    //Methods
    public void swapWith(vec3f vec) 
    {
        vec3f vt = new vec3f(vec);
        vec.setTo(this);
        this.setTo(vt);
    }
    
    public void add(float dx, float dy, float dz) 
    {
        x += dx;
        y += dy;
        z += dz;
    }
    
    public void add(vec3f u) 
    {
        add(u.x, u.y, u.z);
    }

    public void subtract(float dx, float dy, float dz) 
    {
        x -= dx;
        y -= dy;
        z -= dz;
    }

    public void subtract(vec3f vec) 
    {
        subtract(vec.x, vec.y, vec.z);
    }

    public void scalarMult(float scalar) 
    {
        x *= scalar;
        y *= scalar;
        z *= scalar;
    }

    public void invert() 
    {
        x = -x;
        y = -y;
        z = -z;
    }
    
    public void setTo(float vx, float vy, float vz) 
    {
        x = vx;
        y = vy;
        z = vz;
    }

    public void setTo(vec3f u) 
    {
        setTo(u.x, u.y, u.z);
    }

    public void setToZeroVector() 
    {
        setTo(0, 0, 0);
    }

    public void normalize() 
    {
        float rec_length = 1.0f / length();
        x *= rec_length;
        y *= rec_length;
        z *= rec_length;
    }

    public void rotate(vec3f axis, float angleRad) 
    {
        float p = axis.x * Maths.sinRad(angleRad * 0.5f);
        float q = axis.y * Maths.sinRad(angleRad * 0.5f);
        float r = axis.z * Maths.sinRad(angleRad * 0.5f);
        float s = Maths.cosRad(angleRad * 0.5f);
        float[][] matRot = {
            {1 - 2 * q * q - 2 * r * r, 2 * p * q - 2 * s * r, 2 * p * r + 2 * s * q},
            {2 * p * q + 2 * s * r, 1 - 2 * p * p - 2 * r * r, 2 * q * r + 2 * s * p},
            {2 * p * r - 2 * s * q, 2 * q * r - 2 * s * p, 1 - 2 * p * p - 2 * q * q}
        };
        float[][] pForRot = {{x}, {y}, {z}};
        float[][] result = Maths.multMatrix(matRot, pForRot);
        x = result[0][0];
        y = result[1][0];
        z = result[2][0];
    }

    public void rotate(float u, float v, float w, float angleRad) 
    {
        float p = u * Maths.sinRad(angleRad * 0.5f);
        float q = v * Maths.sinRad(angleRad * 0.5f);
        float r = w * Maths.sinRad(angleRad * 0.5f);
        float s = Maths.cosRad(angleRad * 0.5f);
        float[][] matRot = {
            {1 - 2 * q * q - 2 * r * r, 2 * p * q - 2 * s * r, 2 * p * r + 2 * s * q},
            {2 * p * q + 2 * s * r, 1 - 2 * p * p - 2 * r * r, 2 * q * r + 2 * s * p},
            {2 * p * r - 2 * s * q, 2 * q * r - 2 * s * p, 1 - 2 * p * p - 2 * q * q}
        };
        float[][] pForRot = {{x}, {y}, {z}};
        float[][] result = Maths.multMatrix(matRot, pForRot);
        x = result[0][0];
        y = result[1][0];
        z = result[2][0];
    }

    public boolean equals(vec3f u) 
    {
        return (x == u.x && y == u.y && z == u.z);
    }

    public boolean isZero() 
    {
        return (x == 0f && y == 0f && z == 0f);
    }

    public boolean isZeroApprox() 
    {
        return (
            Maths.abs(x) <= equiv
            && Maths.abs(y) <= equiv
            && Maths.abs(z) <= equiv
        );
    }
    
    public boolean isCloseTo(vec3f other) 
    {
        return (
            Maths.abs(x - other.x) <= equiv
            && Maths.abs(y - other.y) <= equiv
            && Maths.abs(z - other.z) <= equiv
        );
    }

    public float length() 
    {
        double x2 = Maths.pow(x, 2);
        double y2 = Maths.pow(y, 2);
        double z2 = Maths.pow(z, 2);
        return (float) Maths.sqrt(x2 + y2 + z2);
    }

    public float lengthSq() 
    {
        float x2 = Maths.sq(x);
        float y2 = Maths.sq(y);
        float z2 = Maths.sq(z);
        return (x2 + y2 + z2);
    }

    public float[] toArray() 
    {
        return new float[] { x, y, z, 1 };
    }

    public String toString() 
    {
        String s = "";
        s += "( " + String.format("%.3f", x) + ", " + String.format("%.3f", y) + ", " + String.format("%.3f", z) + " )";
        return s;
    }

    public vec3f getInverted() 
    {
        return new vec3f(-x, -y, -z);
    }

    public vec3f getUnitVectorCopy() 
    {
        float rec_length = 1.0f / length();
        return new vec3f(x * rec_length, y * rec_length, z * rec_length);
    }
    
    public vec3f getPerpendicVec() 
    {
        if (!this.equals(Vecmath.AXIS_X)) 
        {
            return Vecmath.crossProduct(this, Vecmath.AXIS_X);
        } 
        else if (!this.equals(Vecmath.AXIS_Y)) 
        {
            return Vecmath.crossProduct(this, Vecmath.AXIS_Y);
        } 
        else if (!this.equals(Vecmath.AXIS_Z)) 
        {
            return Vecmath.crossProduct(this, Vecmath.AXIS_Z);
        } 
        else 
        {
            return Vecmath.ORIGIN;
        }
    }

}